/*  --------------------------- (C) COPYRIGHT 2021 Fortiortech ShenZhen -----------------------------
    File Name      : AddFunction.c
    Author         : Fortiortech  Appliction Team
    Version        : V1.0
    Date           : 2021-11-07
    Description    : This file contains .C file function used for Motor Control.
    ----------------------------------------------------------------------------------------------------
                                       All Rights Reserved
    ------------------------------------------------------------------------------------------------- */
#include <MyProject.h>

/* Private variables ---------------------------------------------------------*/
FOCCTRL            xdata   mcFocCtrl;
ADCSample          xdata   AdcSampleValue;
ONVarible         xdata ONOFFTest;
MCRAMP             xdata   MotorControl;
MotorFRTypeDef   xdata  mcFRState;
SLEEPMODE        xdata  SleepSet;
int16              xdata   VSP;
int16              xdata   VSP_LSB;
TIMERTypeDef       xdata   Time;

int16 PFC_IKP_RUN_Temp;
int16 PFC_IKI_RUN_Temp;

int16 Motor_speed = 3600;
int16 Motor_refresh = 0;

void  FGOutput(void);
uint8 a=0;
/*  -------------------------------------------------------------------------------------------------
    Function Name  : Abs_F32
    Description    : 对变量取32位的绝对值
    Date           : 2021-04-10
    Parameter      : value: [输入/出]
    ------------------------------------------------------------------------------------------------- */
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : APP_DIV
    Description    : 将所有用到除法的地方，放在同一个中断，以避免中断串扰
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void APP_DIV(void)
{
    if ( mcPwmInput.PwmDivFlag == 1) //启动除法器，避免与过调值中的除法冲突
    {
        mcPwmInput.PwmDuty = DivQ_L_MDU(mcPwmInput.PwmCompareUpdate, 0x0000, mcPwmInput.PwmArrUpdate);
        mcPwmInput.PwmDivFlag = 0;
    }
}

/* -------------------------------------------------------------------------------------------------
    Function Name  : PWMInputCapture
    Description    : 输入PWM处理
    Date           : 2021-04-10
    Parameter      : None
------------------------------------------------------------------------------------------------- */
uint16 MotorControlVSP = 0;
static uint32 duty_sum=0;
//static uint16 duty_avg=0;
void PWMInputCapture(void)
{
	
    if (mcPwmInput.PwmUpdateFlag == 1) // 有新的duty更新
    {
        if ((Abs_F32(mcPwmInput.PwmCompare - mcPwmInput.PwmCompareOld) < 50) // 两次比较值相近，减少读错率
            && (Abs_F32(mcPwmInput.PwmArrOld - mcPwmInput.PwmArr) < 50)      // 两次周期值相近，减少读错率
            && (mcPwmInput.PwmDivFlag == 0) && (mcPwmInput.PwmDealFlag == 0))
        {
            mcPwmInput.PwmFlag          = 1;                           // PwmFlag置1期间，不读取TIM3__DR和TIM3__ARR;，防止出错
            mcPwmInput.PwmCompareUpdate = (mcPwmInput.PwmCompare >> 1);// 对其乘以32768
            mcPwmInput.PwmArrUpdate     = mcPwmInput.PwmArr;
            mcPwmInput.PwmDivFlag       = 1;                           // 启动除法
            mcPwmInput.PwmFlag          = 0;
            mcPwmInput.PwmDealFlag      = 1;                           // 更新PWM后必须进过一次PWM判断才能重新进入
        }
        
        if ((mcPwmInput.PwmDivFlag == 0) && (mcPwmInput.PwmDealFlag == 1))
        {
            mcPwmInput.PwmDealFlag    = 0;
            /***速度随PWM增大而减小***/
            #if (PWMDUTY_Choose == NegaPWMDUTY)
            {               
                MotorControlVSP = mcPwmInput.PwmDuty;
            }
            /***速度随PWM增大而增大***/
            #elif (PWMDUTY_Choose == PosiPWMDUTY)
            {
							 MotorControlVSP = 32768 - mcPwmInput.PwmDuty;
            }
            #endif
            
            if ((MotorControlVSP > ONPWMDuty))
            {
                MotorControl.FlagONOFF = 1;				
            }
            else if ((MotorControlVSP < OFFPWMDuty))
            {
                MotorControl.FlagONOFF = 0;				
            }
//            duty_sum += MotorControlVSP;
//			MotorControlVSP = duty_sum>>3;
//			duty_sum-=MotorControlVSP;
			
            //转速曲线计算
            if (MotorControl.FlagONOFF == 1)
            {
                if (MotorControlVSP <= MINPWMDuty)
                {
                    MotorControl.TargetValue = Motor_Min_Speed;
                }
                else if (MotorControlVSP < MAXPWMDuty)
                {
				//	MotorControl.TargetValue = Motor_Min_Speed;
                    MotorControl.TargetValue = Motor_Min_Speed + SPEED_K * (MotorControlVSP - MINPWMDuty);
                }
                else
                {
                    MotorControl.TargetValue = Motor_Max_Speed;
                }
            }
            else
            {
                MotorControl.TargetValue = 0;
				
            }
        }
//        MotorControl.TargetValue = Motor_Min_Speed;
        mcPwmInput.PwmUpdateFlag = 0;
        mcPwmInput.PwmCompareOld = mcPwmInput.PwmCompare;//将此次比较值赋值给上次比较值
        mcPwmInput.PwmArrOld     = mcPwmInput.PwmArr;//将此次周期值赋值给上次周期值
    }
    
    /* -----启动除法器，避免与过调值中的除法冲突----- */
    APP_DIV();
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : VSPSample
    Description    : VSP采样
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void VSPSample(void)
{
    /*****VREF的采样获取值并滤波******/
    AdcSampleValue.ADCSpeed = (ADC10_DR);                                    //采集对应端口
    VSP = LPFFunction(AdcSampleValue.ADCSpeed, VSP, 10);                    //注意低通滤波器系数范围为0---127
    if ((VSP > ONPWMDuty) )                       //在ONPWMDuty-OFFPWMDutyHigh之间，电机有转速运行
    {
        MotorControl.FlagONOFF = 1;
    }
    else if ((VSP < OFFPWMDuty))                  //电机停机
    {
        MotorControl.FlagONOFF = 0;		
    }
    
    //转速曲线计算
    if (MotorControl.FlagONOFF == 1) //
    {
        #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
        {
            if (VSP <= MINPWMDuty)   //最小转速运行
            {
                MotorControl.TargetValue = Motor_Min_Speed;
            }
            else if (VSP < MAXPWMDuty) //调速
            {
                MotorControl.TargetValue = Motor_Min_Speed + SPEED_K * (VSP - MINPWMDuty);
				
            }
            else                    //最大转速运行
            {
				
                MotorControl.TargetValue = Motor_Max_Speed;
            }
        }
        #endif
    }
    else
    {
        if (mcFaultSource != FaultNoSource)
        {
            mcFaultSource         = FaultNoSource;
        }
        
        MotorControl.TargetValue = 0;
    }
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : ONOFF_Starttest
    Description    : 启动测试
    Date           : 2021-04-10
    Parameter      : h_test: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void ONOFF_Starttest(void)
{
    if (ONOFFTest.ONOFF_Flag == 1)
    {
        ONOFFTest.ON_Count++;
        
        if (ONOFFTest.ON_Count > StartON_Time)
        {
            ONOFFTest.ON_Count        = 0;
            ONOFFTest.ONOFF_Times++;
            ONOFFTest.ONOFF_Flag      = 0;
            MotorControl.FlagONOFF   = 0;
			
            MotorControl.TargetValue = 0;
        }
    }
    else
    {
        if (mcState != mcFault)
        {
            ONOFFTest.OFF_Count++;
            
            if (ONOFFTest.OFF_Count > StartOFF_Time)
            {
                ONOFFTest.OFF_Count       = 0;
                ONOFFTest.ONOFF_Flag      = 1;
                MotorControl.FlagONOFF   = 1;
                MotorControl.TargetValue = Motor_Min_Speed;
            }
        }
    }
}

/*---------------------------------------------------------------------------*/
/*  Name     :   void TargetSpeed_Colletion(void)
    /* Input    :   NO
    /* Output   :   NO
    /* Description: 获取控制的目标值
    /*---------------------------------------------------------------------------*/
static uint16 CTIBusFlageCnt =0;
static uint16 CTIBusFlageNegCnt=0;
void TargetSpeed_Colletion(void)	//此函数在中断中执行
{
	
    #if (!StartONOFF_Enable)
    {
        #if (SPEED_MODE == PWMMODE)
        {
            /*****VREF的采样获取值并滤波******/
            //            AdcSampleValue.ADCSpeed = (ADC6_DR);                                    //采集对应端口
            //            VSP = LPFFunction(AdcSampleValue.ADCSpeed, VSP, 10);                    //注意低通滤波器系数范围为0---127
            
            //          /***速度随VSP增大而减小***/
            //            #if (PWMDUTY_Choose == NegaPWMDUTY)
            //            {
            //                VSP = 32768 - VSP;
            //            }
            //            #endif
            
            PWMInputCapture();
					
				
            
            /******* 进入睡眠模式**********/
//            if ((mcPwmInput.PwmDuty == 0) || (mcPwmInput.PwmDuty >= 32767))
//            {
//                if ((VSP > ONPWMDuty) && (VSP < OFFPWMDutyHigh))
//                {
//                    VSPSample();
//                }
//                else
//                {
//                    Sleepmode();
//                }
//            }
        }
        #elif (SPEED_MODE == NONEMODE)
        {
					
			#if 0
					MotorControl.FlagONOFF   = 1;	//启动标志
					Motor_refresh ++ ;				//刷新  1ms加1 
					if(Motor_refresh > 5)   		//函数1mS刷新一次，电机速度5ms刷新一次
					{
						Motor_refresh = 0 ;
						if(mcFocCtrl.mcCTIbusFlt < BUS_CURRENT_LIMIT_MAX  && Motor_speed <3600)   //判断空转，空转时转速3600  7000
							{
								Motor_speed++  ;											
							}
						else if(mcFocCtrl.mcCTIbusFlt > BUS_CURRENT_LIMIT_MAX)
							{
								Motor_speed--  ;
							}
					}
					MotorControl.TargetValue = _Q15(Motor_speed / MOTOR_SPEED_BASE); //电流互感器超过一定的电流降低转速
			#else
				MotorControl.FlagONOFF   = 1;//启动标志
				MotorControl.TargetValue = _Q15(500.0 / MOTOR_SPEED_BASE);
			#endif	
//					//转速递加。
//					if(CTIBusFlageCnt>8000){
//						MotorControl.TargetValue = _Q15(3000.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>5000){
//						MotorControl.TargetValue = _Q15(3100.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>4000){
//						MotorControl.TargetValue = _Q15(3300.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>2000){
//						MotorControl.TargetValue = _Q15(3500.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else {
//						MotorControl.TargetValue = _Q15(3600.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}
//					
//					}


//					if(mcFocCtrl.mcCTIbusFlt>BUS_CURRENT_LIMIT){					
//          MotorControl.TargetValue = _Q15(2500.0 / MOTOR_SPEED_BASE); //电流互感器超过一定的电流降低转速
//					CTIBusFlageCnt=5000;
//					GP11 =1;						
//				}else{
//					GP11 =0;
//					if(CTIBusFlageCnt!=0){
//					CTIBusFlageCnt--;
//					}
//					//转速递加。
//					if(CTIBusFlageCnt>4000){
//						MotorControl.TargetValue = _Q15(2000.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>3000){
//						MotorControl.TargetValue = _Q15(2200.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>2000){
//						MotorControl.TargetValue = _Q15(2500.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else if(CTIBusFlageCnt>1000){
//						MotorControl.TargetValue = _Q15(3000.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}else {
//						MotorControl.TargetValue = _Q15(3600.0 / MOTOR_SPEED_BASE); //固定转速赋值
//					}
            
//				}      
        }
        #elif (SPEED_MODE == SREFMODE)
        {
            VSPSample();
        }
        #endif
    }
    #else
    {
        ONOFF_Starttest();
    }
    #endif
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Speed_response
    Description    : 速度响应函数，可根据需求加入控制环，如恒转矩控制、恒转速控制、恒功率控制
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */
void Speed_response(void)
{
	static uint8 IqStartDelay=0;
    /* -----电机启动ATO爬坡函数处理----- */
    StarRampDealwith();
    
    if ((mcState == mcRun) || (mcState == mcStop))
    {
        switch (mcFocCtrl.CtrlMode)
        {
            case 0:
            {
                if (mcFocCtrl.SpeedFlt > Motor_Loop_Speed) //Motor_Loop_Speed>=MOTOR_LOOP_RPM 即切闭环的速度		mcFocCtrl.SpeedFlt=估算器输出值滤波后的值
                {
//                    mcFocCtrl.SpeedModeDelay++;
//                    
//                    if (mcFocCtrl.SpeedModeDelay > 1)
//                    {
                        mcFocCtrl.CtrlMode = 1;				//达到切闭环速度，控制模式1启动
                        FOC_QKP = QKP;
                        FOC_QKI = QKI;
                        FOC_DKP = DKP;
                        FOC_DKI = DKI;
                        FOC_THECOMP    = _Q15(0.0 / 180.0);                    // SMO 估算补偿角
                        #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                        {
                            MotorControl.ActualValue = mcFocCtrl.SpeedFlt;
                        }
                        #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {
                            MotorControl.ActualValue = mcFocCtrl.Powerlpf;	// 功率滤波后的值
                        }
                        #elif (Motor_Speed_Control_Mode == VOLTAGE_LOOP_CONTROL)
                        {
                            MotorControl.ActualValue = mcFocCtrl.mcFOC_UQ;
                        }
                        #endif
                        mcFocCtrl.TorqueLoopTime = SPEED_LOOP_TIME;
                        MotorControl.IncValue = SPEED_INC;	//速度环增量
                        MotorControl.DecValue = SPEED_DEC;	//速度环减量
                        FOC_IDREF   = ID_RUN_CURRENT;     // D轴启动电流
                        mcFocCtrl.mcIqref = IQ_RUN_CURRENT;
                        PI1_UKH = mcFocCtrl.mcIqref;
                    //}
                }
                else
                {
                    mcFocCtrl.SpeedModeDelay = 0;
//									  if (mcFocCtrl.mcIqref < IQ_RUN_CURRENT)
//										{
////											if((++IqStartDelay)>2){
////												IqStartDelay=0;
//												mcFocCtrl.mcIqref +=2;
////											}
//											
//											FOC_IQREF = mcFocCtrl.mcIqref;
//										}
                }
            }
            break;
            
            case 1:
            {
							
//							  FOC_EKP = OBSW_KP_GAIN_RUN4;             // 估算器里的PI的KP
//                FOC_EKI = OBSW_KP_GAIN_RUN4;  
//							  FOC_EKLPFMIN = OBS_EA_KS1;
//							  MotorControl.SMOSpeedMin = FOC_EKLPFMIN;
//                
//                if (MotorControl.SMOSpeedMin > OBS_EA_KS1)
//                {
//                    MotorControl.SMOSpeedMin -= 2;
//                }
//                else
//                {
//                    MotorControl.SMOSpeedMin = OBS_EA_KS1;
//                }
//                
//                FOC_EKLPFMIN = MotorControl.SMOSpeedMin;
                mcFocCtrl.TorqueLoopTime++;		// 转矩环时间
                
                if (mcFocCtrl.TorqueLoopTime > SPEED_LOOP_TIME)
                {
                    mcFocCtrl.TorqueLoopTime = 0;
                    #if (OUTLoop_Mode == OUTLoop_Disable)
                    {
                        mcFocCtrl.mcIqref = FOC_IQREF;
                        
                        if (FOC_IQREF < QOUTMAXVALUE)
                        {
                            mcFocCtrl.mcIqref += QOUTINC;
                            
                            if (mcFocCtrl.mcIqref > QOUTMAXVALUE)
                            {
                                mcFocCtrl.mcIqref = QOUTMAXVALUE;
                            }
                            
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        else if (FOC_IQREF > QOUTMAXVALUE)
                        {
                            mcFocCtrl.mcIqref -= QOUTINC;
                            
                            if (mcFocCtrl.mcIqref < QOUTMAXVALUE)
                            {
                                mcFocCtrl.mcIqref = QOUTMAXVALUE;
                            }
                            
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                    }
                    #elif (OUTLoop_Mode == OUTLoop_Enable)
                    {
					if(mcFocCtrl.Powerlpf>POWER_LIMIT){//功率限制
						if (PI1_UKMAX > SOUTMIN)
             { PI1_UKMAX -= 5; }
					}else{
						if (PI1_UKMAX < SOUTMAX)
             { PI1_UKMAX += 1; }
					}

                    
//                        PI1_KP = SKP;
//                        PI1_KI = SKI;
                        Motor_Ramp();             //电机速度爬坡控制
                        #if (Motor_Speed_Control_Mode == SPEED_LOOP_CONTROL)
                        {
                            mcFocCtrl.mcIqref = HW_One_PI(MotorControl.ActualValue - mcFocCtrl.SpeedFlt);
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        #elif (Motor_Speed_Control_Mode == POWER_LOOP_CONTROL)
                        {
                            mcFocCtrl.mcIqref = HW_One_PI(MotorControl.ActualValue - mcFocCtrl.Powerlpf);
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        #elif (Motor_Speed_Control_Mode == VOLTAGE_LOOP_CONTROL)
                        {
                            mcFocCtrl.mcIqref = HW_One_PI(MotorControl.ActualValue - mcFocCtrl.mcFOC_UQ);
                            FOC_IQREF = mcFocCtrl.mcIqref;
                        }
                        #endif
                    }
                    #endif                               //END OUTLoop_Mode
                }
            }
            break;
        }
    }
    
    #if (FRDetectMethod != FOCMethod)
    {
        if (mcState == mcRun)
        {
            if (mcFocCtrl.RunStateCnt < 1500) //15s的延迟，用于清ISDCCWFlag,BEMFCCWFlag
            {
                mcFocCtrl.RunStateCnt++;
            }
            else
            {
                RSDDetect.RSDCCWFlag   = 0;
            }
        }
    }
    #endif
}

/*  -------------------------------------------------------------------------------------------------
    Function Name  : Motor_Ramp
    Description    :
    Date           : 2021-04-10
    Parameter      : Speedramp: [输入/出]
    ------------------------------------------------------------------------------------------------- */
void Motor_Ramp(void)
{
//    if (--MotorControl.DelayCount < 0)
//    {
//        MotorControl.DelayCount = MotorControl.DelayPeriod;
//        
        if (MotorControl.ActualValue < MotorControl.TargetValue)
        {
            if (MotorControl.ActualValue + MotorControl.IncValue < MotorControl.TargetValue)
            {
                MotorControl.ActualValue += MotorControl.IncValue;
            }
            else
            {
                MotorControl.ActualValue = MotorControl.TargetValue;
            }
        }
        else
        {
            if (MotorControl.ActualValue - MotorControl.DecValue > MotorControl.TargetValue)
            {
                MotorControl.ActualValue -= MotorControl.DecValue;
            }
            else
            {
                MotorControl.ActualValue = MotorControl.TargetValue;
            }
        }
    //}
}
/*  -------------------------------------------------------------------------------------------------
    Function Name  : StarRampDealwith
    Description    :
    Date           : 2021-04-10
    Parameter      : None
    ------------------------------------------------------------------------------------------------- */

void StarRampDealwith(void)
{
    if ((mcState == mcRun) && (mcFocCtrl.CtrlMode == 0))
    {
        if (mcFocCtrl.State_Count == 2400)
        {
			
            FOC_EKP = OBSW_KP_GAIN_RUN;              // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN;              // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 2200)
        {

            FOC_EKP = OBSW_KP_GAIN_RUN1;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN1;             // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 2000)
        {

            FOC_EKP = OBSW_KP_GAIN_RUN2;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN2;             // 估算器里的PI的KI
        }
        else if (mcFocCtrl.State_Count == 1500)
        {
		
            FOC_EKP = OBSW_KP_GAIN_RUN3;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN3;             // 估算器里的PI的KI
        }
				else if (mcFocCtrl.State_Count == 500)
        {
			
            FOC_EKP = OBSW_KP_GAIN_RUN4;             // 估算器里的PI的KP
            FOC_EKI = OBSW_KI_GAIN_RUN4;             // 估算器里的PI的KI
        }
    }
}
void FGOutput(void)
{
    if (mcState == mcRun) 
		{
			if ((FOC__THETA >= -32768) && (FOC__THETA < 0))  // 0-180
			{
				ResetFGPin;
			} 
			else if ((FOC__THETA >= 0) && (FOC__THETA <= 32767))  // 180-360
			{
				SetFGPin;
			}
		} 
		else if (mcState == mcFault) 
		{
			SetFGPin;
		} 
		else 
		{
			ResetFGPin;
		}
}
